
#include <BConearRestraint>
#include "member_BConearRestraint.h"
#include "member_BRestraint.h"
#include "member_BBody.h"
#include "member_BNode.h"

using namespace BWE;

#define member					(*(member_BConearRestraint*)_ptr)
#define member_allocate()		_ptr = new member_BConearRestraint(this)
#define member_release()		delete (member_BConearRestraint*)_ptr

BConearRestraint::BConearRestraint()
{
	member_allocate();
}
BConearRestraint::BConearRestraint(BBody* bodyA, const BVector& pivotA) : BRestraint(bodyA, pivotA)
{
	member_allocate();
}
BConearRestraint::BConearRestraint(BBody* bodyA, BBody* bodyB) : BRestraint(bodyA, bodyB)
{
	member_allocate();
}
BConearRestraint::BConearRestraint(BBody* bodyA, const BVector& pivotA, BBody* bodyB, const BVector& pivotB) : BRestraint(bodyA, pivotA, bodyB, pivotB)
{
	member_allocate();
}
BConearRestraint::~BConearRestraint()
{
	member_release();
}

void BConearRestraint::copy(const BRestraint* other)
{
	BRestraint::copy(other);

	member.twistAngle = conear_member(other)->twistAngle;

	member.twistSpan = conear_member(other)->twistSpan;
	member.swingSpan1 = conear_member(other)->swingSpan1;
	member.swingSpan2 = conear_member(other)->swingSpan2;

	member.biasFactor = conear_member(other)->biasFactor;
	member.damping = conear_member(other)->damping;

	member.axisA = conear_member(other)->axisA;
	member.axisB = conear_member(other)->axisB;
	member.twistLimitRatio = conear_member(other)->twistLimitRatio;

	member.accSwingLimitImpulse = conear_member(other)->accSwingLimitImpulse;
	member.accTwistLimitImpulse = conear_member(other)->accTwistLimitImpulse;
}
BRestraint* BConearRestraint::clone() const
{
	BConearRestraint* restraint = new BConearRestraint();
	restraint->copy(this);
	return restraint;
}

void BConearRestraint::setAxisA(const BVector& axisA)
{
	member.axisA = axisA;
}
const BVector& BConearRestraint::axisA() const
{
	return member.axisA;
}

void BConearRestraint::setAxisB(const BVector& axisB)
{
	member.axisB = axisB;
}
const BVector& BConearRestraint::axisB() const
{
	return member.axisB;
}

void BConearRestraint::setLimit(int limitIndex, BReal limitValue)
{
	switch (limitIndex)
	{
	case 3:
	{
		member.twistSpan = limitValue;
		break;
	}
	case 4:
	{
		member.swingSpan2 = limitValue;
		break;
	}
	case 5:
	{
		member.swingSpan1 = limitValue;
		break;
	}
	default:
	{
	}
	};
}
BReal BConearRestraint::getLimit(int limitIndex) const
{
	switch (limitIndex)
	{
	case 3:
	{
		return member.twistSpan;
		break;
	}
	case 4:
	{
		return member.swingSpan2;
		break;
	}
	case 5:
	{
		return member.swingSpan1;
		break;
	}
	default:
	{
		return 0.0;
	}
	};
}

const BMatrix& BConearRestraint::getAFrame() const
{
	return member.frameA;
};
const BMatrix& BConearRestraint::getBFrame() const
{
	return member.frameB;
};

int BConearRestraint::getSolveTwistLimit()
{
	return member.solveTwistLimit;
}

int BConearRestraint::getSolveSwingLimit()
{
	return member.solveSwingLimit;
}

BReal BConearRestraint::getTwistLimitSign()
{
	return member.twistLimitSign;
}

BVector BConearRestraint::GetPointForAngle(BReal fAngleInRadians, BReal fLength) const
{
	BReal xEllipse = cos(fAngleInRadians);
	BReal yEllipse = sin(fAngleInRadians);

	BReal swingLimit = member.swingSpan1;
	if (fabs(xEllipse) > REAL_EPSILON)
	{
		BReal surfaceSlope2 = (yEllipse * yEllipse) / (xEllipse * xEllipse);
		BReal norm = 1 / (member.swingSpan2 * member.swingSpan2);
		norm += surfaceSlope2 / (member.swingSpan1 * member.swingSpan1);
		BReal swingLimit2 = (1 + surfaceSlope2) / norm;
		swingLimit = std::sqrt(swingLimit2);
	}

	BVector vSwingAxis(0, xEllipse, -yEllipse);
	BQuater qSwing(vSwingAxis, swingLimit);
	BVector vPointInRestraintSpace(fLength, 0, 0);
	return qSwing * vPointInRestraintSpace;
}

void BConearRestraint::setTwistSpan(BReal twistSpan)
{
	member.twistSpan = twistSpan;
}
BReal BConearRestraint::twistSpan() const
{
	return member.twistSpan;
}

void BConearRestraint::setSwingSpan1(BReal swingSpan1)
{
	member.swingSpan1 = swingSpan1;
}
BReal BConearRestraint::swingSpan1() const
{
	return member.swingSpan1;
}

void BConearRestraint::setSwingSpan2(BReal swingSpan2)
{
	member.swingSpan2 = swingSpan2;
}
BReal BConearRestraint::swingSpan2() const
{
	return member.swingSpan2;
}

void BConearRestraint::setBiasFactor(BReal biasFactor) const
{
	member.biasFactor = biasFactor;
}
BReal BConearRestraint::biasFactor() const
{
	return member.biasFactor;
}

BReal BConearRestraint::getTwistAngle() const
{
	return member.twistAngle;
}
bool BConearRestraint::isPastSwingLimit()
{
	return member.solveSwingLimit;
}

void BConearRestraint::setDamping(BReal damping)
{
	member.damping = damping;
}
BReal BConearRestraint::damping() const
{
	return member.damping;
}

void BConearRestraint::enableMotor(bool b)
{
	member.motorEnabled = b;
}
bool BConearRestraint::isMotorEnabled() const
{
	return member.motorEnabled;
}
BReal BConearRestraint::getMaxMotorImpulse() const
{
	return member.maxMotorImpulse;
}
bool BConearRestraint::isMaxMotorImpulseNormalized() const
{
	return member.bNormalizedMotorStrength;
}
void BConearRestraint::setMaxMotorImpulse(BReal maxMotorImpulse)
{
	member.maxMotorImpulse = maxMotorImpulse;
	member.bNormalizedMotorStrength = false;
}
void BConearRestraint::setMaxMotorImpulseNormalized(BReal maxMotorImpulse)
{
	member.maxMotorImpulse = maxMotorImpulse;
	member.bNormalizedMotorStrength = true;
}

const BVector& BConearRestraint::horAxis() const
{
	return member.horAxis;
}
const BVector& BConearRestraint::verAxis() const
{
	return member.verAxis;
}
const BVector& BConearRestraint::coneAxis() const
{
	return member.coneAxis;
}
const BVector& BConearRestraint::swingAxis() const
{
	return member.swingAxis;
}
const BVector& BConearRestraint::twistAxis() const
{
	return member.twistAxis;
}

int BConearRestraint::solverSize()
{
	member.calculateAngleInfo();
	int num = 3;
	if (member.solveSwingLimit)
	{
		num++;
	}
	if (member.solveTwistLimit)
	{
		num++;
	}
	return num;
}
void BConearRestraint::soverItems(BSolverItem* solverItems, BReal stepTime)
{
	const BBody* bodyA = restraint_member(this)->bodyA;
	const BBody* bodyB = restraint_member(this)->bodyB;

	const BMatrix& matrixA = node_member(bodyA)->matrix;
	const BMatrix& matrixB = node_member(bodyB)->matrix;

	solverItems[0].contactNormalA.x() = 1;
	solverItems[1].contactNormalA.y() = 1;
	solverItems[2].contactNormalA.z() = 1;
	BVector vecA = matrixA.mult3(restraint_member(this)->pivotA);
	{
		BVector& angular0 = solverItems[0].torqueAxisA;
		BVector& angular1 = solverItems[1].torqueAxisA;
		BVector& angular2 = solverItems[2].torqueAxisA;
		SkewSymmetric(-vecA, angular0, angular1, angular2);
	}

	solverItems[0].contactNormalB.x() = -1;
	solverItems[1].contactNormalB.y() = -1;
	solverItems[2].contactNormalB.z() = -1;
	BVector vecB = matrixB.mult3(restraint_member(this)->pivotB);
	{
		BVector& angular0 = solverItems[0].torqueAxisB;
		BVector& angular1 = solverItems[1].torqueAxisB;
		BVector& angular2 = solverItems[2].torqueAxisB;
		SkewSymmetric(vecB, angular0, angular1, angular2);
	}

	BVector pointA = matrixA * restraint_member(this)->pivotA;
	BVector pointB = matrixB * restraint_member(this)->pivotB;
	BVector offset = pointB - pointA;

	solverItems[0].velocity = offset.x() / stepTime * member.damping;
	solverItems[1].velocity = offset.y() / stepTime * member.damping;
	solverItems[2].velocity = offset.z() / stepTime * member.damping;

	int row = 2;
	if (member.solveSwingLimit)
	{
		row++;
		solverItems[row].torqueAxisA = member.swingAxis;
		solverItems[row].torqueAxisB = -member.swingAxis;
		solverItems[row].velocity= member.swingError / stepTime * member.biasFactor;
		solverItems[row].impulseLowerLimit = 0;
		solverItems[row].impulseUpperLimit = (member.motorEnabled && member.maxMotorImpulse >= 0.0f) ? member.maxMotorImpulse : REAL_MAX;
	}
	if (member.solveTwistLimit)
	{
		row++;
		solverItems[row].torqueAxisA = member.twistAxis;
		solverItems[row].torqueAxisB = -member.twistAxis;
		solverItems[row].velocity = member.twistError / stepTime * member.biasFactor;
		if (member.twistSpan > 0)
		{
			if (member.twistError> 0)
			{
				solverItems[row].impulseLowerLimit = 0;
				solverItems[row].impulseUpperLimit = REAL_MAX;
			}
			else
			{
				solverItems[row].impulseLowerLimit = -REAL_MAX;
				solverItems[row].impulseUpperLimit = 0;
			}
		}
		else
		{
			solverItems[row].impulseLowerLimit = -REAL_MAX;
			solverItems[row].impulseUpperLimit = REAL_MAX;
		}
	}
}
